/*
 * GoalStrategy.cpp
 *
 */

#include "../../include/vision/GoalStrategy.h"
#include "../../include/Config.h"

namespace objpreproc {

GoalStrategy::GoalStrategy() {
	_erosion_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE,
			cv::Size(2 * GOAL_EROSION_SIZE + 1, 2 * GOAL_EROSION_SIZE + 1),
			cv::Point(GOAL_EROSION_SIZE, GOAL_EROSION_SIZE));

	_dilatation_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE,
			cv::Size(2 * GOAL_DILATATION_SIZE + 1, 2 * GOAL_DILATATION_SIZE + 1),
			cv::Point(GOAL_DILATATION_SIZE, GOAL_DILATATION_SIZE));

	_threshold_values_min = new cv::Scalar(GOAL_H_MIN_0, GOAL_S_MIN_1,
			GOAL_V_MIN_2, 0);

	_threshold_values_max = new cv::Scalar(GOAL_H_MAX_0, GOAL_S_MAX_1,
			GOAL_V_MAX_2, 0);

	_detection_params = new cv::SimpleBlobDetector::Params();
	_detection_params->filterByArea = true;
	_detection_params->filterByConvexity = false;
	_detection_params->filterByCircularity = false;
	_detection_params->filterByInertia = false;
	_detection_params->filterByColor = true;
	_detection_params->minArea = GOAL_MIN_AREA;
	_detection_params->minDistBetweenBlobs = GOAL_MIN_DISTANCE_BETWEEN_BLOBS;
	_detection_params->maxArea = GOAL_MAX_AREA;
	_detection_params->blobColor = GOAL_BLOB_COLOR;
	_simple_blob_detector = new cv::SimpleBlobDetector(*_detection_params);
}

GoalStrategy::~GoalStrategy() {
	_erosion_kernel.release();
	_dilatation_kernel.release();
	delete _threshold_values_min;
	delete _threshold_values_max;
	delete _detection_params;
	delete _simple_blob_detector;

	// Prevenzione del dangling reference.
	_threshold_values_min = NULL;
	_threshold_values_max = NULL;
	_detection_params = NULL;
	_simple_blob_detector = NULL;
}

void GoalStrategy::doThreshold(const cv::Mat &src, cv::Mat &dest) {
	assert(!src.empty());
	assert(!dest.empty());
	assert(src.size == dest.size);

	cv::inRange(src, *_threshold_values_min, *_threshold_values_max, dest);
}

void GoalStrategy::doErosion(const cv::Mat &src, cv::Mat &dest) {
	assert(!src.empty());
	assert(!dest.empty());
	assert(src.size == dest.size);
	assert(src.channels() == dest.channels());

	cv::erode(src, dest, _erosion_kernel);
}

void GoalStrategy::doDilatation(const cv::Mat &src, cv::Mat &dest) {
	assert(!src.empty());
	assert(!dest.empty());
	assert(src.size == dest.size);
	assert(src.channels() == dest.channels());

	cv::dilate(src, dest, _dilatation_kernel);
}

void GoalStrategy::doBlobDetection(const cv::Mat &image, std::vector<cv::KeyPoint> &keypoints, const cv::Mat &mask) {
	assert(!image.empty());

	_simple_blob_detector->detect(image, keypoints, mask);

}

void GoalStrategy::doPOIExtraction(const cv::Mat &depth, std::vector<cv::KeyPoint> &keypoints, std::vector<poi::POI*> &pois) {
	assert(!depth.empty());

	for(unsigned int i = 0; i < keypoints.size(); i++) {
		pois.push_back(utils::Utils::KeyPoint2POIfromDepthMat(keypoints[i], GOAL, depth));
	}

	/*
	if (keypoints.size() > 0) {
		std::vector<cv::KeyPoint>::iterator iter;
		if (keypoints.size() == 1)
			*iter = keypoints.back();
		else
			iter = std::max_element(keypoints.begin(), keypoints.end(),
					boost::lambda::bind(&cv::KeyPoint::response, boost::lambda::_1) < boost::lambda::bind(&cv::KeyPoint::response, boost::lambda::_2));
		pois.push_back(utils::Utils::KeyPoint2POIfromDepthMat(*iter.base(), GOAL, depth));
	}*/
}

} /* namespace objpreproc */
